#ifndef __MGGS_POSSION__
#define __MGGS_POSSION__

#include "Element.h"
#include "Mesh.h"
#include <cmath>
#include <vector>
#include <eigen/Eigen/Sparse>
#include <eigen/Eigen/IterativeLinearSolvers>


double function_u(const Point<double,2> &_P);
double function_f(const Point<double,2> &_P);
VectorXd Possion_Easymesh(EasyMesh& mesh, const int _acc, const double tol);
double useless_error(EasyMesh& mesh, VectorXd &solution);
double prior_error(EasyMesh& mesh, VectorXd &solution, int _acc);
void plots_matlab(EasyMesh& mesh, VectorXd &solution);


double function_u(const Point<double,2> &_P){
  return std::sin(_P.get_coord(0))*std::sin(_P.get_coord(1));
}

double function_f(const Point<double,2> &_P){
  return 2*function_u(_P);
}

VectorXd Possion_Easymesh(EasyMesh& mesh, const int _acc, const double tol){
  TD_P1Element<double> ref_ele;
  ref_ele.read_quad_info("data/triangle.tmp_geo", _acc);
  int n_nodes = mesh.get_n_nodes();
  int n_grids = mesh.get_n_grids();
  int dofs = 3;
  SparseMatrix<double> A(n_nodes,n_nodes);
  VectorXd f;
  f.setZero(n_nodes);
  std::vector<Triplet<double>> TriList(n_grids * n_nodes * n_nodes);
  std::vector<Triplet<double>>::iterator it = TriList.begin();
  for(int e = 0 ; e < n_grids ; e++ ){
    const Vector<int,3>& Point_index = mesh.get_grid(e);
    for (int i = 0 ; i < dofs ; i++)
      ref_ele.set_global_dofs(i,mesh.get_point(Point_index(i)));
    const std::valarray<Point<double,2>> &quad_pnts = ref_ele.get_quad_points();
    const std::valarray<double> &quad_weights = ref_ele.get_quad_weights();	
    int n_quad_pnts = ref_ele.get_n_quad_points();
    double V = 0.5;
    double detJ = ref_ele.global_to_local_det();
    Matrix<double,2,2> invJ = ref_ele.local_to_global_Jacobi();
    for (int q = 0 ; q < n_quad_pnts ; q++){
      for (int i = 0; i < dofs; i++){
	Vector<double,2> grad_i = invJ * ref_ele.local_basis_gradient(i);
	double phi_i = ref_ele.local_basis_function(i, quad_pnts[q]);
	for (int j = 0; j < dofs; j++){
	  Vector<double,2> grad_j = invJ * ref_ele.local_basis_gradient(j);
	  double cont = (grad_i(0) * grad_j(0) + grad_i(1) * grad_j(1)) * quad_weights[q] * detJ * V;
	  *it = Triplet<double>(Point_index(i), Point_index(j), cont);
	  it++;
	}
	f[Point_index(i)] += function_f(ref_ele.local_to_global(quad_pnts[q])) * phi_i * quad_weights[q] * detJ * V;
      }
    }
  }
  A.setFromTriplets(TriList.begin(), TriList.end());
  A.makeCompressed();
  for (int i = 0 ; i < n_nodes ; i++){
    int bm = mesh.get_point(i).get_boundary_mark();
    if (bm == 0)
      continue;
    else if (bm == 1){
      double boundary_value = function_u(mesh.get_point(i));
      f[i] = boundary_value * A.coeffRef(i, i);
      for (SparseMatrix<double>::InnerIterator it(A, i); it; ++it){
	int row = it.row();
	if (row == i)
	  continue;
        f[row] -= A.coeffRef(i, row) * boundary_value;
	A.coeffRef(i, row) = 0.0;
	A.coeffRef(row, i) = 0.0;
      }
    }
  }
  ConjugateGradient<SparseMatrix<double> > Solver_sparse;
  Solver_sparse.setTolerance(tol);
  Solver_sparse.compute(A);
  VectorXd solution= Solver_sparse.solve(f);
  return solution;
}

double useless_error(EasyMesh& mesh, VectorXd &solution){
  int n_nodes = mesh.get_n_nodes();
  double err = 0.0;
  for (int i = 0; i < n_nodes; i++)
    err += (solution(i) - function_u(mesh.get_point(i))) * (solution(i) - function_u(mesh.get_point(i)));
  err = std::sqrt(err);
  return err;
}

double prior_error(EasyMesh& mesh, VectorXd &solution, int _acc){
  TD_P1Element<double> ref_ele;
  ref_ele.read_quad_info("data/triangle.tmp_geo", _acc);
  int n_nodes = mesh.get_n_nodes();
  int n_grids = mesh.get_n_grids();
  int dofs = 3;
  double sum = 0.0;
  for(int e = 0 ; e < n_grids ; e++ ){
    const Vector<int,3>& Point_index = mesh.get_grid(e);
    for (int i = 0 ; i < dofs ; i++)
      ref_ele.set_global_dofs(i,mesh.get_point(Point_index(i)));
    const std::valarray<Point<double,2>> &quad_pnts = ref_ele.get_quad_points();
    const std::valarray<double> &quad_weights = ref_ele.get_quad_weights();	
    int n_quad_pnts = ref_ele.get_n_quad_points();
    double V = 0.5;
    double detJ = ref_ele.global_to_local_det();
    for (int q = 0 ; q < n_quad_pnts ; q++){
      double a = solution(Point_index(0));
      double b = solution(Point_index(1));
      double c = solution(Point_index(2));
      double tmp = a+(b-a)*quad_pnts[q].get_coord(0)+(c-a)*quad_pnts[q].get_coord(1)
	- function_u(ref_ele.local_to_global(quad_pnts[q]));
      sum+= tmp * tmp * quad_weights[q] * detJ * V;
    }
  }
  sum = std::sqrt(sum);
  return sum;
}

void plots_matlab(EasyMesh& mesh, VectorXd &solution){
  int n_nodes = mesh.get_n_nodes();
  int n_grids = mesh.get_n_grids();
  std::ofstream os;
  os.open("result.m");
  os << "subplot(1,2,1)\n";
  os << "[x0,y0] = meshgrid(1:0.02:2);\n";
  os << "z0 = sin(x0).*sin(y0);\n";
  os << "mesh(x0,y0,z0),xlabel('x'),ylabel('y'),zlabel('z');title('reference solution');\n";
  os << "subplot(1,2,2)\n";
  os << "res = [\n";
  for (int i = 0; i < n_nodes; i++){
    os << mesh.get_point(i).get_coord(0) << "," << mesh.get_point(i).get_coord(1) << ","
       << solution(i) << ";\n";
  }
  os << "];\n";
  os << "T = [\n";
  for (int i = 0; i < n_grids; i++){
    const Vector<int,3> &Index = mesh.get_grid(i);
    os << Index(0)+1 << "," << Index(1)+1 << "," << Index(2)+1 << ";\n";
  }
  os << "];\n";
  os << "x=res(:,1);y=res(:,2);z=res(:,3);\n";
  os << "trimesh(T,x,y,z);,xlabel('x'),ylabel('y'),zlabel('z');title('calculation result');";
  os.close();
}



#else
// DO NOTHING.
#endif
